/**
 * *****************************************************************
 * @file    adt3102_dsp.c
 * @author  WuHao(hwu@andartechs.com.cn)
 * @version 1.0.0
 * @date    2020-11-09
 * @brief
 *
 *                 Copyright (c) 2020, Andar Technologies Inc.
 *                           www.andartechs.com 
 *
 * *****************************************************************
 */
/*------------------------------ include -------------------------------------*/
#include "math.h"
#include "adt3102_type_define.h"
#include "adt3102.h"
#include "stdio.h"
#include "adt3102_dsp.h"
#include "adt3102_dma.h"
#include "adt3102_gpio.h"
#include "dsp_ctype_map.h"
#include "dma_ctype_map.h"


/*------------------------------ variable ------------------------------------*/
volatile int32 v_g_fftFlag;


/*------------------------------ function ------------------------------------*/
/**
 * @brief   ADT301 ASIC FFT.
 *          |   1   2 3 4 5..........................................fftLen|
            srcAddr
 * @param   fftLen: fft points.
 * @param   srcAddr:fft source data first address.
 * @param   srcStep:fft source data step.
 * @param   dstStep:fft destination data step.
 * @return  None.
 * @note
 */
void fftSingle(int32 fftLen, uint32 srcAddr, uint32 dstAddr, uint16 srcStep, uint16 dstStep)
{
    uint32 regVal;
    DSP->dsp_fft_ctrl = dsp_fft_ctrl_fft_exp_bp_bit | dsp_fft_ctrl_fft_en_bit | fftLen;
    DSP->dsp_int_clear = dsp_int_clear_fft_start_bit;
    v_g_fftFlag = 1;
    dmaConfig(DMA0, srcAddr, FFT_BUF_HF1, srcStep, 4, fftLen/2, PERI_NONE, MEMTOMEM);  
    while(1)
    {
        regVal = DMA->dma_0_tcnt;
        if(regVal >= fftLen/2)
        {
            //printf("The adc_mem TO first_fft_mem OK \r\n");
            break;
        }
    }
    dmaDisable(DMA0);
    dmaConfig(DMA0, srcAddr+fftLen/2*srcStep, FFT_BUF_HF2, srcStep, 4, fftLen/2, PERI_NONE, MEMTOMEM);  
    while(1)
    {
        regVal = DMA->dma_0_tcnt;
        if(regVal >= fftLen/2)
        {
            //printf("The adc_mem TO first_fft_mem OK \r\n");
            break;
        }
    }
//    //software write fft
//    v_g_fftFlag = 1;
//    for(int32 i=0; i<fftLen/2;i++)
//    {
//        *(int32 *)(FFT_BUF_HF1+i*4)=*(int32 *)(srcAddr+srcStep*i);
//        //*(int32 *)(FFT_BUF_HF2+i*4)=*(int32 *)(srcAddr+fftLen*2+srcStep*i);
//    }    
//    //has to finish 1st half !
//    for(int32 i=0; i<fftLen/2;i++)
//    {
//        *(int32 *)(FFT_BUF_HF2+i*4)=*(int32 *)(srcAddr+fftLen/2*srcStep+srcStep*i);
//    }
    while( 1 == v_g_fftFlag); // printf("1D fft done ! \r\n");
    dmaDisable(DMA0);
    dmaConfig(DMA0, FFT_BUF_HF1, dstAddr, 4, dstStep, fftLen/2, PERI_NONE, MEMTOMEM);
    while(1)
    {
        regVal = DMA->dma_0_tcnt;
        if(regVal >= fftLen/2)
        {
            //printf("The adc_mem TO first_fft_mem OK \r\n");
            break;
        }
    }
    dmaDisable(DMA0);
    dmaConfig(DMA0, FFT_BUF_HF2, dstAddr+fftLen/2*dstStep, 4, dstStep, fftLen/2, PERI_NONE, MEMTOMEM);
    while(1)
    {
        regVal = DMA->dma_0_tcnt;
        if(regVal >= fftLen/2)
        {
            //printf("The adc_mem TO first_fft_mem OK \r\n");
            break;
        }
    }
    dmaDisable(DMA0);
//    //software write fft
//    for(int32 i=0; i<fftLen/2;i++)
//    {
//        *(int32 *)(dstAddr+dstStep*i)=*(int32 *)(FFT_BUF_HF1+i*4);
//        //*(int32 *)(dstAddr+fftLen*2+dstStep*i)=*(int32 *)(FFT_BUF_HF2+i*4);
//    }    
//    for(int32 i=0; i<fftLen/2;i++)
//    {
//        *(int32 *)(dstAddr+fftLen/2*dstStep+dstStep*i)=*(int32 *)(FFT_BUF_HF2+i*4);
//    }
    v_g_fftFlag = 1;
}


/**
 * @brief   Energy merging after FFT.
 * @param   None.
 * @return  None.
 * @note
 */
void absAdt3102(uint16 fftLen ,uint16 rngMax ,uint32 srcAddr,uint32 dstAddr)
{
    //Incoherent accumulation
    int32 l = 0;
    for(int32 i =0; i<fftLen*rngMax*2; i=i+2)
    {
        int16 tmp_r = (*( ((int16 *)srcAddr) + i ));
        int16 tmp_i = (*( (int16 *)srcAddr + i+1 ));
        *( ((int32 *)dstAddr) + l )=tmp_r*tmp_r+tmp_i*tmp_i;
        l++;
    }
}


/**
 * @brief   ADT301 ASIC FFT.
 * srcAddr |11 12 13 14 15..........................................1(fftLen) |
 *      |21 22 23 24 25..........................................2(fftLen) |
 *      |31 32 33 34 35..........................................3(fftLen) |
 *      |..................................................................|
 *      |..................................................................|
 *      |..................................................................|
 *      |..................................................................|
 *      |..................................................................|
 *      |(countFft)1 (countFft)2 (countFft)3.............(countFft)(fftLen)|
 * @param   fftLen:.
 * @param   srcAddr:.
 * @param   srcStep:.
 * @param   dstStep:.
 * @return  None.
 * @note
 */
void fftProcess(int32 fftLen, int32 fftChirpNum, uint32 srcAddr, uint32 dstAddr, uint16 srcStep, uint16 dstStep)
{
    //DSP->dsp_fft_ctrl = dsp_fft_ctrl_fft_exp_bp_bit | fftLen;
    for(int32 fftFrameCount=0; fftFrameCount<fftChirpNum; fftFrameCount++)
    {
        fftSingle(fftLen, srcAddr+fftFrameCount*fftLen*4, dstAddr+fftFrameCount*fftLen*4, srcStep, dstStep);
    }  
    DSP->dsp_fft_ctrl &= ~dsp_fft_ctrl_fft_en_bit;
}


/**
 * @brief   Do two-dimensional FFT.
 * @param   dopFftLen: doppler fft points.
 * @param   rngMax: maximum on range dimension.
 * @param   srcAddr: doppler fft source data first address.
 * @param   dstAddr: doppler fft destination data first address.
 * @param   srcStep: doppler fft source data step.
 * @param   dstStep: doppler fft destination data step.
 * @return  None.
 * @note
 */
void velocityFftProcess(int32 dopFftLen, int32 rngMax, uint32 srcAddr, uint32 dstAddr, uint16 srcStep, uint16 dstStep)
{
    #ifdef VEL_FFT_WN_EN
    int16 wnIdxStep;
    switch(dopFftLen)
    {
        case 64:
            wnIdxStep = 0;
            break;
        case 32:
            wnIdxStep = 1;
            break;
        case 16:
            wnIdxStep = 2;
            break;
        default :
            wnIdxStep = 3;
    }
    #endif
    //DSP->dsp_fft_ctrl = dsp_fft_ctrl_fft_exp_bp_bit | fftLen;
    for(int32 fftFrameCount=0; fftFrameCount<rngMax; fftFrameCount++)
    {        
        #ifdef VEL_FFT_WN_EN
        velFftWn(dopFftLen, srcAddr+fftFrameCount*4,srcStep,wnIdxStep);
        #endif
        fftSingle(dopFftLen, srcAddr+fftFrameCount*4, dstAddr+fftFrameCount*dopFftLen*4, srcStep, dstStep);
        //fftSingle(dopFftLen, srcAddr+fftFrameCount*4, dstAddr+fftFrameCount*4, srcStep, dstStep);
    }  
    DSP->dsp_fft_ctrl &= ~dsp_fft_ctrl_fft_en_bit;
}


/**
 * @brief   serach for single peak.
 * @param   dopFftLen: doppler fft points.
 * @param   rngMax: maximum on range dimension.
 * @param   absMergeArray: An array that holds energy vals.
 * @return  None.
 * @note
 */
void dopplerSwap(int32 dopFftLen, int32 rngMax, uint32 (*absMergeArray)[CHIRP_NUM]);
void dopplerSwap(int32 dopFftLen, int32 rngMax, uint32 (*absMergeArray)[CHIRP_NUM])
{
    uint32 tempdata2Dfft1,tempdata2Dfft2;
	for(uint32 count1 =0; count1< rngMax; count1++)
    {
        for(unsigned char count2 =0; count2< dopFftLen/2; count2++)
        {
            //swap velocity.After swap, absMergeArray[][dopFftLen/2] is static 
            tempdata2Dfft1  = absMergeArray[count1][count2];
            tempdata2Dfft2  = absMergeArray[count1][count2+dopFftLen/2];
            absMergeArray[count1][count2] = tempdata2Dfft2;
            absMergeArray[count1][count2+dopFftLen/2] = tempdata2Dfft1 ;
        }
    }
}

/**
 * @brief   velocity hanning window process by software
        |   1   2 3 4 5..........................................fftLen|
        srcAddr.
 * @param   fftLen: fft length <=64.
 * @param   srcAddr: .
 * @param   srcStep: .
 * @param   wnIdxStep: window table index step.
 * @return  None.
 * @note
 */
void velFftWn(int32 fftLen, uint32 srcAddr, uint16 srcStep, uint16 wnIdxStep)
{
    //hanning window *2^13
    uint16 w_hn[64]={2,10,21,38,59,84,113,146,182,221,263,308,354,402,450,500,549,598,647,694,739,782,823,861,895,926,953,976,995,1009,1019,1023,1023,1019,1009,995,976,953,926,895,861,823,782,739,694,647,598,549,500,450,402,354,308,263,221,182,146,113,84,59,38,21,10,2};
    volatile int32* pData=NULL;
    int32 iqRead;
    int16 iRead,qRead;
    int32 iWn,qWn;
    int16 wnIdx;
    pData = (volatile int32*)(srcAddr);    
    for(uint16 i = 0; i < fftLen; i++)
    {
        iqRead = *pData;
        iRead  = iqRead & (0xffff);
        qRead  = iqRead >> 16;
        wnIdx = i<<wnIdxStep;
        iWn = iRead*w_hn[wnIdx];
        qWn = qRead*w_hn[wnIdx];
        iRead = (int16)(iWn>>9); // shift 9 instead of 10 to compensate 6db power loss on signal
        qRead = (int16)(qWn>>9);
        *pData = (qRead<<16) + iRead;
        pData = (volatile int32*)((int32)pData + (int32)srcStep);
    }
}

